#include "servoControl.h"
#include <glog/logging.h>
#include <QFileInfo>
#include <QSettings>
#include <QTime>
#include <QDir>
#include <thread>
#include "logger.h"

// 根据脉冲数和实际距离的关系决定
// 横轴 10w 脉冲 = 199.89mm
double mm2PulseForX(double value)
{
    return (100000.0 / 249.8673) * value;
}

double Pulse2mmForX(double value)
{
    return (249.8673 / 100000.0) * value;
}

// 纵轴 10W 脉冲 = 179.9 mm
double mm2PulseForY(double value)
{
    return (100000.0 / 179.9) * value;
}

double Pulse2mmForY(double value)
{
    return (179.9 / 100000.0) * value;
}

servoControl::servoControl()
{
    loadINI();
    pingger.init(servoIP);
    x_max = mm2PulseForX(x_max);
    y_max =  mm2PulseForY(y_max);
}

servoControl::~servoControl()
{
    smc_board_close(m_nConnectNo);
}

bool servoControl:: connectCtrler(int &axisNum)
{
    if(true == mInited){
        return true;
    }

    print_base_info();
    axisNum = 0;
    short iret = smc_board_init(m_nConnectNo, SMC_USE_NETWORK, servoIP, 115200);    //网络链接
    if(iret != 0)
    {
        LOG(ERROR) << "smc_board_init failed, iret = "<<iret <<"; servoIP = " <<servoIP <<"\n";
        return false;
    }
    LOG(ERROR) << "axisNum = "<<axisNum;

    unsigned long errcode=0;
    nmcs_get_errcode(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM, &errcode);    //EtherCAT 总线端口号，固定为 2(其中 0、1 为 CANopen 端口)
    if(errcode != 0)
    {
         LOG(ERROR) << "errcode = "<<errcode;
        for(int i=0;i<3;i++){
            clearErr();
            nmcs_get_errcode(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM, &errcode);
            if(errcode == 0){
                break;
            }else{
                LOG(ERROR) << "smc_soft_reset  errcode " << errcode;
                smc_soft_reset(m_nConnectNo);
                std::this_thread::sleep_for(std::chrono::milliseconds(10*1000));
                short iret = smc_board_init(m_nConnectNo, SMC_USE_NETWORK, servoIP, 115200);
                if(iret != 0)
                {
                    LOG(ERROR) << "Servo Connect Failed!!! iret = "<<iret<<"\n";
                    return false;
                }
            }
        }
        nmcs_get_errcode(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM, &errcode);
        if(errcode != 0)
        {
            LOG(ERROR) << "Exceptions Exist on BUS!!! errcode ="<<errcode<<"\n";
            return false;
        }
    }
    nmcs_get_total_axes(m_nConnectNo, &TotalAxis);
    axisNum = TotalAxis;
    LOG(INFO) << TotalAxis << " Axes Found!!!\n";

    mInited = true;
    return mInited;
}

void servoControl::clearErr()
{
    nmcs_set_alarm_clear(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM, 0);   // 清除报警信号

    // 清除总线错误
    nmcs_clear_card_errcode(m_nConnectNo);
    nmcs_clear_errcode(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM);

    for(int i=0;i<TotalAxis;i++){
        nmcs_clear_axis_errcode(m_nConnectNo, i);
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
}

bool servoControl::enableAllAxis(bool toEnable)
{
    bool ret = true;
    unsigned long errcode=0;
    unsigned short statemachine=0;

    // 检查总线异常
    nmcs_get_errcode(m_nConnectNo, SMC_ERR_ETHERCAT_PORT_NUM, &errcode);
    if(errcode != 0) //总线正常才允许使能操作
    {
        LOG(ERROR) << "Bus Error!!!\n";
        return false;
    }

    if(!toEnable)
    {
        // 下使能
        for(int i = 0; i < (int)TotalAxis; i++)
        {
            nmcs_set_axis_disable(m_nConnectNo, i);
            is_Enable[i] = false;
        }
        ret = true;
    }else{
        // 上使能
        for(int i = 0 ; i <  (int)TotalAxis; i++){
            //如果已经上使能，不操作
            nmcs_get_axis_state_machine(m_nConnectNo, i, &statemachine);
            if(statemachine == 4){
                LOG(INFO) << i << " enable!\n";
                is_Enable[i] = true;
                continue;
            }

            for(int j = 0; j < 3; j++){
                nmcs_set_axis_enable(m_nConnectNo, i);
                std::this_thread::sleep_for(std::chrono::milliseconds(500));

                nmcs_get_axis_state_machine(m_nConnectNo, i, &statemachine);
                if(statemachine == 4){
                    LOG(INFO) << i << " enable success!\n";
                    is_Enable[i] = true;
                    ret = true;
                    break;
                }
                else{
                    LOG(ERROR) << i << " enable failed!! try again!!\n";
                    is_Enable[i] = false;
                    ret = false;
                }
            }
        }
    }
    return ret;
}

void servoControl::stopMove()
{
    short iret = 0;
    for(int i=0;i<TotalAxis;i++){
        iret = smc_set_dec_stop_time(m_nConnectNo, i, 0.01);    //设置减速停止时间，单位s
        iret = smc_stop(m_nConnectNo, i, 0);                    //减速停止

        iret = smc_set_dec_stop_time(m_nConnectNo, i, 0.01);    //设置减速停止时间，单位s
        iret = smc_stop(m_nConnectNo, i, 0);                    //减速停止
    }

}

int servoControl::set_output_pin(int pin_num, int value)
{
   short sRet = smc_write_outbit(m_nConnectNo,pin_num,value);
   LOG(INFO) << "sRet : " << sRet << std::endl;
   return sRet;
}

int servoControl::get_input_pin(int pin_num, int &value)
{
    short sRet = smc_read_inbit(m_nConnectNo, pin_num);
    value = sRet ;
    return 0;
}

int servoControl::close()
{
    smc_board_close(m_nConnectNo);
    return 0;
}

int servoControl::reset()
{
    smc_soft_reset(m_nConnectNo);
    std::this_thread::sleep_for(std::chrono::milliseconds(10*1000));
    return 0;
}

int servoControl::getConnectStatus()
{
    return smc_get_connect_status(m_nConnectNo);
}

bool servoControl::runMoveHomeX()
{
    if(mCheckZeroInit && !is_ZeroInit[0]){
        LOG(ERROR) << "is_ZeroInit[0] false" <<std::endl;
        return false;
    }

    //X轴移动到一个准备位置
    bool ret = runSingleAxis(0, x_home_pos, MOVE_ABSOLUTE);
    if(!ret){
        return false;
    }
    return true;
}

bool servoControl::runMoveHomeY()
{
    if(mCheckZeroInit && !is_ZeroInit[0]){
        LOG(ERROR) << "is_ZeroInit[0] false" <<std::endl;
        return false;
    }

    //Y轴回零
    bool ret = runBackZero(1);
    if(!ret){
        return false;
    }
    return true;
}

void servoControl::print_base_info()
{
    LOG(INFO)<<"*******************servoControl********************\n";
    LOG(INFO) << "X home pose: " << x_home_pos << std::endl;
    LOG(INFO) << "X move speed: " << move_speed[0] << std::endl;
    LOG(INFO) << "Y move speed: " << move_speed[1] << std::endl;
    LOG(INFO) << "servo IP: " << servoIP << std::endl;
    LOG(INFO) << "m_nConnectNo: " << m_nConnectNo << std::endl;
    LOG(INFO)<<"***************************************************\n";
}

bool servoControl::runBackZero(int axis)
{
    LOG(INFO) << "Zero mode: " << zero_mode[axis] << std::endl;

    is_ZeroInit[axis] = false;

    short iret = 0;

    // 已上使能，执行回零
    if(is_Enable[axis])
    {
        // 设置速度曲线
        //                                                  回零低速  回零高速   加速时间  减速时间
        iret = smc_set_home_profile_unit(m_nConnectNo, axis, 2000, zero_speed, 0.1, 0.1);

        // 设置回零方式
        iret = smc_set_homemode(m_nConnectNo, axis, 0, 0, zero_mode[axis], 0);

        // 启动回零运动
        iret = smc_home_move(m_nConnectNo, axis);
        if(iret != 0)
        {
            LOG(ERROR) << "smc_home_move failed!! err" << iret <<"\n";
            return false;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(200));

        while(smc_check_done( m_nConnectNo, axis ) == 0)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        WORD state = 0;
        iret = smc_get_home_result(m_nConnectNo, axis, &state);
        if(state==1)
        {
            LOG(INFO) << "axis " << axis << " to zeroPos success!!\n";
            is_ZeroInit[axis] = true;
            return true;
        }
        else
        {
            LOG(ERROR) << "axis " << axis << " to zeroPos failed! errCode: " << iret << std::endl;
            return false;
        }
    }

    return false;
}

bool servoControl::runMoveHome()
{
    if(mCheckZeroInit && !is_ZeroInit[0]){
        LOG(ERROR) << "is_ZeroInit[0] false" <<std::endl;
        return false;
    }

    bool ret = false;
    if(TotalAxis>1){
        //Y轴回零
        ret = runBackZero(1);
        if(!ret){
            return false;
        }
    }

    //X轴移动到一个准备位置
    ret = runSingleAxis(0, x_home_pos, MOVE_ABSOLUTE);
    if(!ret){
        return false;
    }
    return true;
}

/*脉冲当量就是 代表每移动1mm(单位：脉冲数/mm)，需要发送的脉冲数； 定长运动的距离参数是脉冲数    */
bool servoControl::runSingleAxis(int axis, int pulses, int posMode)
{

#if 1
//    if(mCheckZeroInit && !is_ZeroInit[axis]){
//        LOG(ERROR) << "is_ZeroInit " << axis <<" false" <<std::endl;
//        return false;
//    }

    //软保护，不超过物理限定距离
//    if(0 == axis ){
//        if(pulses< x_max){
//            pulses = x_max;
//        }
//    }

//    if(1 == axis ){
//        if(pulses > y_max){
//            pulses = y_max;
//        }
//    }
    LOG(INFO) << "pulses " << pulses ;
    short iret = 0;
    // 设置脉冲当量
    iret = smc_set_equiv(m_nConnectNo, axis, 1);

    // 设定脉冲模式（此处脉冲模式固定为P+D方向：脉冲+方向）
    iret = smc_set_pulse_outmode(m_nConnectNo, axis, 0);

    // 设定单轴运动速度参数  起始速度 最大速度 加速时间 减速时间 停止速度
    iret = smc_set_profile_unit(m_nConnectNo, axis, 0, 5000, 0.1, 0.1, 0);

    // 设定S段时间
    iret = smc_set_s_profile(m_nConnectNo, axis, 0, 0);

    // posMode =0 相对运动  posMode = 1 绝对位置运动
    LOG(INFO) << "posMode" << posMode;
    LOG(INFO) << "pulses" << pulses;
    iret = smc_pmove_unit(m_nConnectNo, axis, pulses, posMode);
    if(iret != 0)
    {
        LOG(ERROR) << "axis " << axis << " operate move failed!! errCode:" << iret << "\n\n";
        return false;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(200));

    LOG(INFO) << "axis " << axis << " start moving...\n";
    while(smc_check_done( m_nConnectNo, axis ) == 0)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
    LOG(INFO) << "axis " << axis << " move success!!!\n";
#endif
    return true;
}

bool servoControl::getPosition(int axis, double &pos)
{
    unsigned long errcode=0;
    nmcs_get_errcode(m_nConnectNo, 2, &errcode);
    if(errcode != 0)
        return false;

    pos = 0.0;
    smc_get_position_unit( m_nConnectNo, axis, &pos);        // 获取当前轴位置
    return true;
}

void servoControl::loadINI()
{
    QFileInfo fileInfo(CONFIG_FILE_PATH);
    if(fileInfo.isFile())
    {
        QSettings setting(CONFIG_FILE_PATH, QSettings::IniFormat);
        setting.setIniCodec("utf-8");

        std::string ip = setting.value("/SERVO/servoIP").toString().toStdString();
        memset(servoIP, 0, sizeof (servoIP));
        sprintf(servoIP, "%s", ip.c_str());

        x_home_pos  = setting.value("/SERVO/PosX_Home").toInt();
        move_speed[0]   = setting.value("/SERVO/moveSpeedX").toInt();
        move_speed[1]   = setting.value("/SERVO/moveSpeedY").toInt();
        LOG(INFO) << "loadINI  x_home_pos = " << x_home_pos << std::endl;

        x_home_pos = mm2PulseForX(x_home_pos);
    }
}
